/**
  ****************************(C) COPYRIGHT 2019 DJI****************************
  * @file       can_receive.c/h
  * @brief      there is CAN interrupt function  to receive motor data,
  *             and CAN send function to send motor current to control motor.
  *             这里是CAN中断接收函数，接收电机数据,CAN发送函数发送电机电流控制电机.
  * @note       
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Dec-26-2018     RM              1. done
  *  V1.1.0     Nov-11-2019     RM              1. support hal lib
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2019 DJI****************************
  */

#include "CAN_receive.h"

#include "cmsis_os.h"

#include "main.h"
#include "bsp_rng.h"


#include "detect_task.h"

extern CAN_HandleTypeDef hcan1;
extern CAN_HandleTypeDef hcan2;
//motor data read
/**
 * @brief 获取电机测量数据
 * 
 * @param ptr 指向电机测量数据结构体的指针
 * @param data 接收到的CAN数据
 */
#define get_motor_measure(ptr, data)                                \
  {                                                                 \
    (ptr)->last_ecd = (ptr)->ecd;                                   \
    (ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]);            \
    (ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]);      \
    (ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]);  \
    (ptr)->temperate = (data)[6];                                   \
  }

/**
 * @brief 从CAN数据中获取超级电容器的测量值
 * 
 * @param ptr 指向超级电容器测量值结构体的指针
 * @param data CAN数据数组
 */
#define get_supercap_measure(ptr, data)                                  \
  {                                                                      \
    (ptr)->battery_voltage = (uint16_t)((data)[0] << 8 | (data)[1]);     \
    (ptr)->capacitance_voltage = (uint16_t)((data)[2] << 8 | (data)[3]); \
    (ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]);       \
    (ptr)->capacitance_percentage = (data)[6];                           \
    (ptr)->power_set = (data)[7];                                        \
  }

//电机数据
motor_measure_t motor_chassis[4];
motor_measure_t motor_gimbal[5];
sup_measure_t sup_cap[1];


static CAN_TxHeaderTypeDef  gimbal_tx_message;
static uint8_t              gimbal_can_send_data[8];
static CAN_TxHeaderTypeDef  chassis_tx_message;
static uint8_t              chassis_can_send_data[8];
static CAN_TxHeaderTypeDef  shoot_tx_message;
static uint8_t              shoot_can_send_data[8];
static CAN_TxHeaderTypeDef  sup_tx_message;
static uint8_t              sup_can_send_data[2];


/**
  * @brief          hal库CAN回调函数,接收can电机数据
  * @param[in]      hcan:CAN句柄指针
  * @retval         none
  */
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
	CAN_RxHeaderTypeDef rx_header;
	uint8_t rx_data[8] = {0};

	/* 底盘电机与拨弹电机 */
	if(hcan == &CHASSIS_CAN)
	{
		HAL_CAN_GetRxMessage(&CHASSIS_CAN, CAN_RX_FIFO0, &rx_header, rx_data);
		switch (rx_header.StdId)
		{
			case CAN_3508_M1_ID:
			case CAN_3508_M2_ID:
			case CAN_3508_M3_ID:
			case CAN_3508_M4_ID:
			{
				static uint8_t i = 0;
				//get motor id
				i = rx_header.StdId - CAN_3508_M1_ID;
				get_motor_measure(&motor_chassis[i], rx_data);
				detect_hook(CHASSIS_MOTOR1_TOE + i);
				break;
			}
			case CAN_TRIGER_MOTOR_ID:
			{
				get_motor_measure(&motor_gimbal[4], rx_data);
				detect_hook(TRIGGER_MOTOR_TOE);
				break;
			}        
			case CAN_SUPERCAP_RX_ID:
			{
				get_supercap_measure(&sup_cap[0], rx_data);
			//  detect_hook(SUPERCAP_TOE);
				break;
			}
		
			default:
			{
				break;
			}
		}
	}
	/* 云台电机 */
	else if(hcan == &GIMBAL_CAN)
	{
		HAL_CAN_GetRxMessage(&GIMBAL_CAN, CAN_RX_FIFO0, &rx_header, rx_data);
		switch (rx_header.StdId)
		{        
			case CAN_YAW_MOTOR_ID:
			case CAN_PIT_MOTOR_ID:
			case CAN_FRIC1_ID:
			case CAN_FRIC2_ID:
			{
				static uint8_t i = 0;
				//get motor id
				i = rx_header.StdId - 0x205;
				get_motor_measure(&motor_gimbal[i], rx_data);
				detect_hook(YAW_GIMBAL_MOTOR_TOE + i);
				break;
			}

			default:
			{
					break;
			}
		}
	}
}

/**
  * @brief          send control current of motor (0x205, 0x206, 0x207, 0x208)
  * @param[in]      yaw: (0x205) 6020 motor control current, range [-30000,30000] 
  * @param[in]      pitch: (0x206) 6020 motor control current, range [-30000,30000]
  * @param[in]      shoot: (0x207) 2006 motor control current, range [-10000,10000]
  * @param[in]      rev: (0x208) reserve motor control current
  * @retval         none
  */
/**
  * @brief          发送电机控制电流(0x205,0x206,0x207,0x208)
  * @param[in]      yaw: (0x205) 6020电机控制电流, 范围 [-30000,30000]
  * @param[in]      pitch: (0x206) 6020电机控制电流, 范围 [-30000,30000]
  * @param[in]      fric1: (0x207) 3508电机控制电流, 范围 [-10000,10000]
  * @param[in]      fric2: (0x208) 3508电机控制电流, 范围 [-10000,10000]
  * @retval         none
  */
void CAN_cmd_gimbal(int16_t yaw ,int16_t pitch, int16_t fric1 ,int16_t fric2)
{
    uint32_t send_mail_box;
    gimbal_tx_message.StdId = CAN_GIMBAL_ALL_ID;
    gimbal_tx_message.IDE = CAN_ID_STD;
    gimbal_tx_message.RTR = CAN_RTR_DATA;
    gimbal_tx_message.DLC = 0x08;

    gimbal_can_send_data[0] = yaw >> 8;
    gimbal_can_send_data[1] = yaw;
    gimbal_can_send_data[2] = pitch >> 8;
    gimbal_can_send_data[3] = pitch;
    gimbal_can_send_data[4] = fric1 >> 8;
    gimbal_can_send_data[5] = fric1;
    gimbal_can_send_data[6] = fric2 >> 8;
    gimbal_can_send_data[7] = fric2;

    HAL_CAN_AddTxMessage(&GIMBAL_CAN, &gimbal_tx_message, gimbal_can_send_data, &send_mail_box);
}

/**
  * @brief          send CAN packet of ID 0x700, it will set chassis motor 3508 to quick ID setting
  * @param[in]      none
  * @retval         none
  */
/**
  * @brief          发送ID为0x700的CAN包,它会设置3508电机进入快速设置ID
  * @param[in]      none
  * @retval         none
  */
void CAN_cmd_chassis_reset_ID(void)
{
    uint32_t send_mail_box;
    chassis_tx_message.StdId = 0x700;
    chassis_tx_message.IDE = CAN_ID_STD;
    chassis_tx_message.RTR = CAN_RTR_DATA;
    chassis_tx_message.DLC = 0x08;
    chassis_can_send_data[0] = 0;
    chassis_can_send_data[1] = 0;
    chassis_can_send_data[2] = 0;
    chassis_can_send_data[3] = 0;
    chassis_can_send_data[4] = 0;
    chassis_can_send_data[5] = 0;
    chassis_can_send_data[6] = 0;
    chassis_can_send_data[7] = 0;

    HAL_CAN_AddTxMessage(&CHASSIS_CAN, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}


/**
  * @brief          send control current of motor (0x201, 0x202, 0x203, 0x204)
  * @param[in]      motor1: (0x201) 3508 motor control current, range [-16384,16384] 
  * @param[in]      motor2: (0x202) 3508 motor control current, range [-16384,16384] 
  * @param[in]      motor3: (0x203) 3508 motor control current, range [-16384,16384] 
  * @param[in]      motor4: (0x204) 3508 motor control current, range [-16384,16384] 
  * @retval         none
  */
/**
  * @brief          发送电机控制电流(0x201,0x202,0x203,0x204)
  * @param[in]      motor1: (0x201) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      motor2: (0x202) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      motor3: (0x203) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      motor4: (0x204) 3508电机控制电流, 范围 [-16384,16384]
  * @retval         none
  */
void CAN_cmd_chassis(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4)
{
    uint32_t send_mail_box;
    chassis_tx_message.StdId = CAN_CHASSIS_ALL_ID;
    chassis_tx_message.IDE = CAN_ID_STD;
    chassis_tx_message.RTR = CAN_RTR_DATA;
    chassis_tx_message.DLC = 0x08;
    chassis_can_send_data[0] = motor1 >> 8;
    chassis_can_send_data[1] = motor1;
    chassis_can_send_data[2] = motor2 >> 8;
    chassis_can_send_data[3] = motor2;
    chassis_can_send_data[4] = motor3 >> 8;
    chassis_can_send_data[5] = motor3;
    chassis_can_send_data[6] = motor4 >> 8;
    chassis_can_send_data[7] = motor4;

    HAL_CAN_AddTxMessage(&CHASSIS_CAN, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
}

//拨弹轮电机控制电流，M2006电机(5号)
/**
 * @brief Sends a command to shoot using CAN communication.
 *
 * @param motor5 The speed of motor 1.
 * @param motor6 The speed of motor 2.
 * @param motor7 The speed of motor 3.
 * @param rev Reserved.
 */
void CAN_cmd_shoot(int16_t shoot ,int16_t rev1, int16_t rev2, int16_t rev3)
{ 
    uint32_t send_mail_box;
    shoot_tx_message.StdId = CAN_GIMBAL_ALL_ID;
    shoot_tx_message.IDE = CAN_ID_STD;
    shoot_tx_message.RTR = CAN_RTR_DATA;
    shoot_tx_message.DLC = 0x08;
    shoot_can_send_data[0] = shoot >> 8;
    shoot_can_send_data[1] = shoot;
    shoot_can_send_data[2] = rev1 >> 8;
    shoot_can_send_data[3] = rev1;
    shoot_can_send_data[4] = rev2 >> 8;
    shoot_can_send_data[5] = rev2;
    shoot_can_send_data[6] = rev3 >> 8;
    shoot_can_send_data[7] = rev3;
    HAL_CAN_AddTxMessage(&CHASSIS_CAN, &shoot_tx_message, shoot_can_send_data, &send_mail_box);
}

/**
 * @brief 发送CAN命令以控制超级电容器电源。
 * 
 * 此函数设置发送CAN消息以控制超级电容器电源所需的参数。
 * 它配置CAN消息的ID、格式、类型、数据长度和数据负载。
 * power_set参数指定超级电容器的期望功率级别。
 * 
 * @param power_set 要设置给超级电容器的功率级别，单位0.01W，限额30-120W。
 */
/**
 * @brief Sends a CAN command to control the supercapacitor power.
 * 
 * This function sets up the necessary parameters for sending a CAN message to control the supercapacitor power.
 * It configures the CAN message ID, format, type, data length, and data payload.
 * The power_set parameter specifies the desired power level for the supercapacitor.
 * 
 * @param power_set The power level to be set for the supercapacitor.
 */
void CAN_cmd_supercap(int16_t power_set)
{
  uint32_t send_mail_box;
	power_set = power_set *100;
  sup_tx_message.StdId = CAN_SUPERCAP_TX_ID;
  sup_tx_message.IDE = CAN_ID_STD;
  sup_tx_message.RTR = CAN_RTR_DATA;
  sup_tx_message.DLC = 0x02;
  sup_can_send_data[0] = power_set >> 8;
  sup_can_send_data[1] = power_set;
  HAL_CAN_AddTxMessage(&CHASSIS_CAN, &sup_tx_message, sup_can_send_data, &send_mail_box);
}

/**
  * @brief          return the yaw 6020 motor data point
  * @param[in]      none
  * @retval         motor data point
  */
/**
  * @brief          返回yaw 6020电机数据指针
  * @param[in]      none
  * @retval         电机数据指针
  */
const motor_measure_t *get_yaw_gimbal_motor_measure_point(void)
{
    return &motor_gimbal[0];
}

/**
  * @brief          return the pitch 6020 motor data point
  * @param[in]      none
  * @retval         motor data point
  */
/**
  * @brief          返回pitch 6020电机数据指针
  * @param[in]      none
  * @retval         电机数据指针
  */
const motor_measure_t *get_pitch_gimbal_motor_measure_point(void)
{
    return &motor_gimbal[1];
}


/**
  * @brief          return the trigger 2006 motor data point
  * @param[in]      none
  * @retval         motor data point
  */
/**
  * @brief          返回拨弹电机 2006电机数据指针
  * @param[in]      none
  * @retval         电机数据指针
  */
const motor_measure_t *get_trigger_motor_measure_point(void)
{
    return &motor_gimbal[4];
}

/**
  * @brief          返回底盘电机 3508电机数据指针
  * @param[in]      i: 电机编号,范围[0,3]
  * @retval         电机数据指针
  */
const motor_measure_t *get_chassis_motor_measure_point(uint8_t i)
{
    return &motor_chassis[(i & 0x03)];
}

/**
 * 获取左射击电机的测量数据指针
 *
 * @return 左射击电机测量数据的指针
 */
const motor_measure_t *get_fric1_motor_measure_point(void)
{
  return &motor_gimbal[2];
}

/**
 * @brief 获取右射击电机的测量数据
 * 
 * @return 右射击电机的测量数据指针
 */
const motor_measure_t *get_fric2_motor_measure_point(void)
{
  return &motor_gimbal[3];
}

/**
 * @brief 获取超级电容器的测量数据
 * 
 * @return 超级电容器的测量数据指针
 */
const sup_measure_t *get_supercap_measure_point(void)
{
  return &sup_cap[0];
}
